/*
  SOLID - Interference Detection Library
  Copyright (C) 1997 Gino van den Bergen

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Library General Public
  License as published by the Free Software Foundation; either
  version 2 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Library General Public License for more details.

  You should have received a copy of the GNU Library General Public
  License along with this library; if not, write to the Free
  Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

  Please send remarks, questions and bug reports to gino@win.tue.nl,
  or write to:
                  Gino van den Bergen
		  Department of Mathematics and Computing Science
		  Eindhoven University of Technology
		  P.O. Box 513, 5600 MB Eindhoven, The Netherlands
*/

#include "BBoxTree.h"

#include <algo.h>

#include "Polygon.h"
#include "Plane.h"

Plane select(int npolys, const Polygon **polys) {
  int axis = BBox(npolys, polys).longestAxis();
	printf("SELEZIONAMENTO Plane - inizio\n");
    printf("axis: %d\n", axis);   
  
  Scalar median = 0;
  for (int i = 0; i < npolys; ++i) {
    Scalar mid = 0;
    for (int j = 0; j < polys[i]->size(); ++j)  
      mid += (*polys[i])[j][axis];
    median += mid / polys[i]->size();
  }
    printf("nmesh: %d\n", npolys);   
    printf("median: %g\n", median);   
    printf("SELEZIONAMENTO Plane - fine\n");
  return Plane(axis == X, axis == Y, axis == Z, median / npolys);
}

BBoxTree::BBoxTree(int n, const Polygon **p) : bound(n, p) {

  Plane plane = select(n, p);
  printf("COSTRUTTORE BoxTree - inizio\n");
  printf("DIST: %g\n",plane.distance());
  
  int white = 0, blue = n;
  while (white < blue) {
    Scalar b, e;
    p[white]->interval(plane, b, e);
    if (b + e < 0) ++white;
    else swap(p[white], p[--blue]);
  }
  
  printf("WHITE: %d\n", white);   
  if (blue == 0 || blue == n) {

    tag = LEAF;
    leaf.polys = new const Polygon *[n];
    copy(&p[0], &p[n], &leaf.polys[0]);
    leaf.npolys = n;
  }
  else {
    printf("BLUE: %d\n", blue);   
    tag = INTERNAL;
    internal.left = new BBoxTree(white, &p[0]);
    internal.right = new BBoxTree(n - blue, &p[blue]);
  }
    printf("COSTRUTTORE BoxTree - fine\n");
}

BBoxTree::~BBoxTree() {
  if (isLeaf()) delete [] leaf.polys;
  else {
    delete internal.left;
    delete internal.right;
  }
}


bool intersect(const BBoxTree& tree1, const BBoxTree& tree2, 
	       const Transform& t, const Matrix& a) { 

  if (!intersect(tree1.bound, tree2.bound, t, a)) {
  FILE *fp;
  fp=fopen("intersect.txt", "a+");
			fprintf(fp, "box NON si intersecano\n");
  fclose(fp);
                              return false;
                              }
		else {
               FILE *fp;
  fp=fopen("intersect.txt", "a+");
fprintf(fp, "box si intersecano\n");
  fclose(fp);
             
             }

  
  if (tree1.isLeaf() && tree2.isLeaf()) {
  FILE *fp;
  fp=fopen("intersect.txt", "a+");
		fprintf(fp, "entrambe foglie\n");
  fclose(fp);
    for (int j = 0; j < tree2.leaf.npolys; ++j) {

    const Polygon* poly1 = tree2.leaf.polys[j];
    Plane plneq1 = (poly1)->plane();
    Point* verts1 = poly1->getVertex();
    Vector norm1 = (&plneq1)->normal();
    Scalar dist1 = (&plneq1)->distance();
  fp=fopen("intersect.txt", "a+");
    fprintf(fp, "poly\nnverts: %d\n",(poly1)->size());
    fprintf(fp, "verts: %g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n",verts1[0][0],verts1[0][1],verts1[0][2],verts1[1][0],verts1[1][1],verts1[1][2],verts1[2][0],verts1[2][1],verts1[2][2],verts1[3][0],verts1[3][1],verts1[3][2]);
 fprintf(fp, "plane.norm:\t%g\t%g\t%g\nplane.dist:\t%g\n",norm1[X],norm1[Y],norm1[Z],dist1);
  fclose(fp);

    const Point pos = (&t)->getPosition();
    const Matrix orn = (&t)->getOrientation();
    // printf("pos: %g\t%g\t%g\n",pos[0],pos[1],pos[2]);
    // printf("orn: %g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n",orn[0][0],orn[0][1],orn[0][2],orn[1][0],orn[1][1],orn[1][2],orn[2][0],orn[2][1],orn[2][2]);

      Polygon poly(*tree2.leaf.polys[j], t);

    poly1 = (&poly);
    plneq1 = (poly1)->plane();
    verts1 = poly1->getVertex();
    norm1 = (&plneq1)->normal();
    dist1 = (&plneq1)->distance();
    // printf("poly\nnverts: %d\n",(poly1)->size());
    // printf("verts: %g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n%g\t%g\t%g\n",verts1[0][0],verts1[0][1],verts1[0][2],verts1[1][0],verts1[1][1],verts1[1][2],verts1[2][0],verts1[2][1],verts1[2][2],verts1[3][0],verts1[3][1],verts1[3][2]);
    // printf("plane.norm:\t%g\t%g\t%g\nplane.dist:\t%g\n",norm1[X],norm1[Y],norm1[Z],dist1);

      for (int i = 0; i < tree1.leaf.npolys; ++i)
	if (intersect(*tree1.leaf.polys[i], poly)) {
       // printf("hello world :D %d %d",i,j);
  FILE *fp;
  fp=fopen("intersect.txt", "a+");
		fprintf(fp, "entrambe foglie - i poligoni si intersecano. Collisione verificata.\n");
  fclose(fp);
       return true;
       }
  FILE *fp;
  fp=fopen("intersect.txt", "a+");
		fprintf(fp, "entrambe foglie - nessun poligono si interseca\n");
  fclose(fp);
      return false;
    }
  }
  else if (tree1.isLeaf() || 
	   (!tree2.isLeaf() && tree1.bound.size() < tree2.bound.size())) {
  FILE *fp;
  fp=fopen("intersect.txt", "a+");
		fprintf(fp, "non entrambe foglie\n");
  fclose(fp);
    return intersect(tree1, *tree2.internal.left, t, a) || 
      intersect(tree1, *tree2.internal.right, t, a); 
  }
  FILE *fp;
  fp=fopen("intersect.txt", "a+");
		fprintf(fp, "intersezione con figli tree1\n");
  fclose(fp);

  return intersect(*tree1.internal.left, tree2, t, a) || 
    intersect(*tree1.internal.right, tree2, t, a); 
}

void intersect(const BBoxTree& tree1, const BBoxTree& tree2, 
	       const Transform& t, const Matrix& a, PointBuf& report) { 
  if (!intersect(tree1.bound, tree2.bound, t, a)) return;
  
  if (tree1.isLeaf() && tree2.isLeaf()) {
    for (int j = 0; j < tree2.leaf.npolys; ++j) {
      Polygon poly(*tree2.leaf.polys[j], t);
      for (int i = 0; i < tree1.leaf.npolys; ++i)
	intersect(*tree1.leaf.polys[i], poly, report);
    }
  }
  else if (tree1.isLeaf() || 
	   (!tree2.isLeaf() && tree1.bound.size() < tree2.bound.size())) {
    intersect(tree1, *tree2.internal.left, t, a,report);
    intersect(tree1, *tree2.internal.right, t, a, report); 
  }
  else {
    intersect(*tree1.internal.left, tree2, t, a, report);
    intersect(*tree1.internal.right, tree2, t, a, report); 
  }
}

